package com.rockwell.sniffyHunter.test;

import java.util.Map;
import java.util.HashMap;

import junit.framework.Assert;

import android.annotation.SuppressLint;
import com.rockwell.sniffyhunter.model.MoveToNextZoneObject;
import com.rockwell.sniffyhunter.model.Robot;
import com.rockwell.sniffyhunter.utils.DetectionIndex;
import com.rockwell.sniffyhunter.utils.IDetector;
import com.rockwell.sniffyhunter.utils.IRobotCommander;
import com.rockwell.sniffyhunter.utils.IRobotSensors;
import com.rockwell.sniffyhunter.utils.MoveToNextZoneState;
import com.rockwell.sniffyhunter.utils.Vector2D;

public class MoveToNextZoneStateTester extends UnitTestLooper {
	private IRobotSensors 			sensor;
	private IRobotCommander 		robotCommander;
	private FakeTreasureCollector 	treasureCollector;
	private Robot 					robot; 
	private MoveToNextZoneObject	moveToNextZoneObj;
	private Map<Integer, IDetector> colorDetectors;
	private FakeTreasureDetector	treasureDetector;
	
	@SuppressLint("UseSparseArrays") @SuppressWarnings("serial")
	@Override
	public void setUp() {
		sensor 				= new FakeRobotSensors();
		robotCommander 		= new DummyRobotCommander();
		treasureCollector 	= new FakeTreasureCollector();
		treasureDetector	= new FakeTreasureDetector();
		colorDetectors 		= new HashMap<Integer, IDetector>() {
							  {
								  put(DetectionIndex.TREASURE, treasureDetector);
								  put(DetectionIndex.OBSTACLE, new FakeColorDetector()/*obstacle detector*/); 
								  put(DetectionIndex.MAZE1, new FakeColorDetector()/*maze1 detector*/);
								  put(DetectionIndex.MAZE2, new FakeColorDetector()/*maze2 detector*/);
								  put(DetectionIndex.MAZE3, new FakeColorDetector()/*maze3 detector*/);
								  put(DetectionIndex.DROPZONE, new FakeColorDetector()/*dropZone detector*/);
								  put(DetectionIndex.MAZE_EXIT, new FakeColorDetector()/*dropZone detector*/);
							  }};
		robot 				= new Robot(0, 0,
								1 /*start Zone*/,
								sensor, 
								robotCommander, 
								null/*maze*/, 
								null/*solver*/, 
								colorDetectors,
								treasureCollector);
		moveToNextZoneObj 	= new MoveToNextZoneObject(robot);
	}
	
	@Override
	public void tearDown() {

	}
	
	public void testDestinationReached() {
		Assert.assertEquals(moveToNextZoneObj.getStateMachine(), MoveToNextZoneState.START);
		moveToNextZoneObj.PerformAction();
		Assert.assertEquals(moveToNextZoneObj.getStateMachine(), MoveToNextZoneState.ROTATE_TO_DESTINATION);
		
		loopUntilCondition(moveToNextZoneObj, MoveToNextZoneState.MOVE_BY_OFFSET);
		Assert.assertEquals(moveToNextZoneObj.getStateMachine(), MoveToNextZoneState.MOVE_BY_OFFSET);
		
		loopUntilCondition(moveToNextZoneObj, MoveToNextZoneState.START);
		Assert.assertEquals(moveToNextZoneObj.getStateMachine(), MoveToNextZoneState.START);
		Assert.assertTrue(moveToNextZoneObj.IsDestinationReached());
	}
	
	public void testEncounteredObstacleOnTheWay() {
		Assert.assertEquals(moveToNextZoneObj.getStateMachine(), MoveToNextZoneState.START);
		moveToNextZoneObj.PerformAction();
		Assert.assertEquals(moveToNextZoneObj.getStateMachine(), MoveToNextZoneState.ROTATE_TO_DESTINATION);
		
		loopUntilCondition(moveToNextZoneObj, MoveToNextZoneState.MOVE_BY_OFFSET);
		moveToNextZoneObj.PerformAction();
		Assert.assertEquals(moveToNextZoneObj.getStateMachine(), MoveToNextZoneState.MOVE_BY_OFFSET);
		
		// TODO: need to find a way to cause the commandSequencer to Abort
		//loopUntilCondition(moveToNextZoneObj, MoveToNextZoneState.INDEX_ROTATE);
		//Assert.assertEquals(moveToNextZoneObj.getStateMachine(), MoveToNextZoneState.INDEX_ROTATE);
		
		//loopUntilCondition(moveToNextZoneObj, MoveToNextZoneState.MOVE_BY_OFFSET);
		//Assert.assertEquals(moveToNextZoneObj.getStateMachine(), MoveToNextZoneState.MOVE_BY_OFFSET);
	}
	
	public void testPositiveAngleAndDistanceToOffset() {
		Vector2D currPos = new Vector2D(1, 1, 45);
		Vector2D destPos = new Vector2D(2, 0, 180);
		
		int angleOffset = Vector2D.calculateAngleTo(currPos, destPos);
		double distanceOffset = Vector2D.calculateDistanceBetween(currPos, destPos);
		currPos.angleFromNorth += angleOffset;
		int remainingAngleOffset = Vector2D.calculateAngleBetween(currPos, destPos);
		currPos.angleFromNorth += remainingAngleOffset;
		
		Assert.assertEquals(90, angleOffset);
		Assert.assertEquals(Math.sqrt(2), distanceOffset);
		Assert.assertEquals(45, remainingAngleOffset);
		Assert.assertEquals(180, currPos.angleFromNorth);
	}
	
	public void testNegativeAngleAndDistanceToOffset() {
		Vector2D currPos = new Vector2D(1, 1, 45);
		Vector2D destPos = new Vector2D(0, 2, 180);
		
		int angleOffset = Vector2D.calculateAngleTo(currPos, destPos);
		currPos.angleFromNorth += angleOffset;
		double distanceOffset = Vector2D.calculateDistanceBetween(currPos, destPos);
		int remainingAngleOffset = Vector2D.calculateAngleBetween(currPos, destPos);
		currPos.angleFromNorth += remainingAngleOffset;
		
		Assert.assertEquals(-90, angleOffset);
		Assert.assertEquals(Math.sqrt(2), distanceOffset);
		Assert.assertEquals(225, remainingAngleOffset);
		Assert.assertEquals(180, currPos.angleFromNorth);
	}	
}
